function plot_uncertainty_ellipse(xytheta, S, fig_id, colors)


figure(fig_id);
x = xytheta(1); y = xytheta(2);
plot(y, x, [colors(1) '.']);

[v,d] = eig(S(1:2,1:2));
w =  v * sqrt(d);
t = linspace(0,2*pi,20);
ellipse_points = [x + w(1,1) .* cos(t) + w(1,2) .* sin(t); y + w(2,1) .* cos(t) + w(2,2) .* sin(t)]';
plot(ellipse_points(:,2), ellipse_points(:,1) , colors(2));


if(length(xytheta) == 3)
    theta = xytheta(3);
    l = min(sqrt([d(1,1) d(2,2)]));
    arrow3([y, x], [y, x] + [sin(theta), cos(theta)]* l,colors(1), l/30, l/5);
    arrow3([y, x], [y, x] + [sin(theta + sqrt(S(3,3))), cos(theta + sqrt(S(3,3)))]* l,colors(2), l/30, l/5);
    arrow3([y, x], [y, x] + [sin(theta - sqrt(S(3,3))), cos(theta - sqrt(S(3,3)))]* l,colors(2), l/30, l/5);
end

axis equal;
